#include "stdafx.h"
#include "HandDetector3D.h"


HandDetector3D::HandDetector3D(GLFWwindow * pWindow)
{
	renderer.initialize(pWindow,DRAW_CROP_128);
	costFunction.initialize(&renderer,&oclManager);
	swarm.swCostFunction = &costFunction;

	cv::FileStorage fs("depthConvertTable.yml", cv::FileStorage::READ);
	_ASSERT(fs.isOpened());
	fs["convertTable_s"] >> depthConvertTable;
	fs.release();

	kinect_focal_length = KINECT_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS;
	wristPosGT = cv::Vec3f(0,-70, 20);
}

HandDetector3D::~HandDetector3D(void)
{
}

void HandDetector3D::setObservation(cv::Mat obsDepth, cv::Mat obsColor, cv::Vec3d centerPosition,BOOL needPredictWrist)
{
	int depthPos = int(centerPosition[2]-kinect_focal_length);
	_ASSERT(depthPos>0);

	double frushtum_near = KINECT_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS;
	double frushtum_far = frushtum_near+1000;
	double fn_scale = frushtum_far/frushtum_near;

	cv::flip(obsDepth, obsDepth, 0);
	cv::flip(obsColor,obsColor,0);

	for (int i=0; i<obsDepth.rows; i++)
	{
		for (int j=0; j<obsDepth.cols; j++)
		{
			ushort depth_var = obsDepth.at<ushort>(i,j);
			size_t buf_position = i*SIZE_IMAGE_PARTICLE_WIDTH+j;
			if (depth_var != 0x0000 && depth_var != 0xffff){
				ushort newVal = ushort(fn_scale*0xffff / ( 1000/(depth_var-frushtum_near) + fn_scale-1 ));
				obsDepth.at<ushort>(i,j) = newVal;
			}else{
				obsDepth.at<ushort>(i,j) = depth_var;
			}
		}
	}
	costFunction.oclManager->setObserve((ushort*)obsDepth.data,obsColor.data,depthConvertTable.at<ushort>(0,depthPos));
	
	//Set Frustum_zoom for GLRenderer
	renderer.calculatingHandCenter(centerPosition);
	
	//Estimate WRIST then Set PSO limitation
	if (needPredictWrist)
	{
		cv::Vec3d realWrist = centerPosition + wristPosGT;
		swarm.setWristRange(
			realWrist[0],
			realWrist[1],
			realWrist[2],
			30,30,50);
	}

}

void HandDetector3D::trackingFrame(int maxIteration)
{
	int iteration = 0;
	swarm.startSwarm();
	while (iteration < maxIteration)
	{
		swarm.iteration();
		iteration++;
		if (iteration%3 == 0 && iteration>5)
		{
			double ranPercent = 0.5 + 0.3*(iteration/maxIteration);
			swarm.randomize(ranPercent,6,vPOS_DOF);		
		}
		printf("Best cost of Generation %d: \t %.2f \n",iteration, swarm.current_gBestCost());
		currentOptimize = swarm.current_gBestCost();
	}
}

void HandDetector3D::drawResultToMat(cv::Mat rawImg, cv::Rect rect)
{
	using namespace cv;
	Mat rawCrop = rawImg(rect);
	for (int i=0;i<previewHandImage.rows;i++)
	{
		for (int j=0;j<previewHandImage.cols;j++)
		{
			Vec3b temp = previewHandImage.at<Vec3b>(i,j);
			if (temp != Vec3b(0,0,0))
			{
				rawCrop.at<Vec3b>(i,j)+=temp*0.8;
			}
		}
	}
}

void HandDetector3D::trackingFirstFrame()
{
	//RESET SWARM
	swarm.endSwarm();
	/* 0. Set Particles */
	double potentialPos[22]={
		0,0,1000,	0,0,0,
		50,0,0,0,
		0,0,0,
		0,0,0,
		0,0,0,
		0,0,0};
	vector<double> mPos(potentialPos,potentialPos+22);
	swarm.setPotentialParticleWithSeeder(mPos);
	

	int gen = 100;
	trackingFrame(gen);
	
	lastestPos = swarm.gBestPosition;
	previewHypo = PositionToHypo(lastestPos);
	swarm.setWristRange(lastestPos[0],	lastestPos[1],	lastestPos[2],	25,25,25); //??? hard code =))
	renderer.renderHandModelGL_For_Preview(previewHypo);
}

void HandDetector3D::trackingNextFrame()
{
	//RESET SWARM
	swarm.endSwarm();
	swarm.setPotentialParticleWithSeeder(lastestPos);

	int gen = 30;
	trackingFrame(gen);

	lastestPos = swarm.gBestPosition;
	previewHypo = PositionToHypo(lastestPos);
	swarm.setWristRange(lastestPos[0],	lastestPos[1],	lastestPos[2],	25,25,25); //??? hard code =))
	renderer.renderHandModelGL_For_Preview(PositionToHypo(lastestPos));
}

#ifdef ALGORITHMS_SIMULATE
void HandDetector3D::runTest()
{
	SDKTimer sampleTimer;
	int timex = sampleTimer.createTimer();
	sampleTimer.resetTimer(timex);

	float tempoArray[26]={
		0,0,1000,
		0,0,0,	
		50,60,30,20,	
		0,10,10,10,
		0,15,0,0,	
		0,20,10,0,	
		0,20,10,0};

	Hypothesis obsHypo;
	memcpy(&obsHypo,tempoArray,26*sizeof(float));
	renderer.calculatingHandCenter(cv::Point3f(0,70,1000));
	
	costFunction.simulatingObserve(obsHypo);

	int iteration = 0;
	int maxGeneration = 100;
	ParticleSwarm swarm;
	swarm.setWristRange(0,0,1000,20,40,30);

	swarm.startSwarm(); // Init the first generation
	sampleTimer.startTimer(timex);
	while (iteration < maxGeneration)
	{
		swarm.iteration();
		iteration++;
		if (iteration%3 == 0)
		{
			swarm.randomize(0.5,0,vPOS_DOF);
			printf("Randomize %d \n", iteration/3);
		}
		printf("Best cost of Generation %d: \t %.2f \n",iteration, swarm.current_gBestCost());

		Hypothesis tmp = PositionToHypo(swarm.gBestPosition);
		renderer.renderHandModelGL_Comparing(obsHypo,tmp);
		//printf("DOF = %.2f, %.2f, %.2f\n",tmp.dofData[0],tmp.dofData[1],tmp.dofData[2]);
	}

	sampleTimer.stopTimer(timex);
	double timeCL = sampleTimer.readTimer(timex);
	printf("time = %f\n",timeCL);
	_getch();
}
#endif
